#include <linux/pgtable.h>
#include <riscv/mmu-early.h>
#include <linux/bits.h>
#include <linux/string.h>

/*
 * rv64 PTE format:
 * | 63 | 62 61 | 60 54 | 53  10 | 9             8 | 7 | 6 | 5 | 4 | 3 | 2 | 1 | 0
 *   N      MT     RSV    PFN      reserved for SW   D   A   G   U   X   W   R   V
 */
#define _PAGE_PFN_MASK GENMASK(53, 10)

#define pgd_next_t pte_t
#define create_pgd_next_mapping(__nextp, __va, __pa, __sz, __prot) \
    create_pmd_mapping(__nextp, __va, __pa, __sz, __prot)
#define get_pgd_next_virt(__ops, __pa) (__ops)->get_pte_virt(__pa)
#define alloc_pgd_next(__ops, __va) (__ops)->alloc_pte(__va)

#define __page_val_to_pfn(_val) (((_val) & _PAGE_PFN_MASK) >> _PAGE_PFN_SHIFT)

static void create_pte_mapping(pte_t *ptep, uintptr_t va, phys_addr_t pa, phys_addr_t sz,
                               pgprot_t prot)
{
    uintptr_t pte_idx = pte_index(va);

    if (pte_none(ptep[pte_idx]))
        ptep[pte_idx] = pfn_pte(PFN_DOWN(pa), prot);
}

static inline unsigned long _pgd_pfn(pgd_t pgd)
{
    return __page_val_to_pfn(pgd_val(pgd));
}

static void create_pmd_mapping(pmd_t *pmdp,
                               uintptr_t va, phys_addr_t pa,
                               phys_addr_t sz, pgprot_t prot)
{
    pte_t *ptep;
    phys_addr_t pte_phys;
    uintptr_t pmd_idx = pmd_index(va);

    if (sz == PMD_SIZE)
    {
        if (pmd_none(pmdp[pmd_idx]))
            pmdp[pmd_idx] = pfn_pmd(PFN_DOWN(pa), prot);
        return;
    }
}

void create_pgd_mapping(struct pt_alloc_ops *ptops, pgd_t *pgdp, uintptr_t va, phys_addr_t pa, phys_addr_t sz,
                        pgprot_t prot)
{
    pgd_next_t *nextp;
    phys_addr_t next_phys;
    uintptr_t pgd_idx = pgd_index(va);

    if (sz == PGDIR_SIZE)
    {
        if (pgd_val(pgdp[pgd_idx]) == 0)
            pgdp[pgd_idx] = pfn_pgd(PFN_DOWN(pa), prot);
        return;
    }

    if (pgd_val(pgdp[pgd_idx]) == 0)
    {
        next_phys = alloc_pgd_next(ptops, va);
        pgdp[pgd_idx] = pfn_pgd(PFN_DOWN(next_phys), PAGE_TABLE);
        nextp = get_pgd_next_virt(ptops, next_phys);
        memset(nextp, 0, PAGE_SIZE);
    }
    else
    {
        next_phys = PFN_PHYS(_pgd_pfn(pgdp[pgd_idx]));
        nextp = get_pgd_next_virt(ptops, next_phys);
    }

    create_pgd_next_mapping(nextp, va, pa, sz, prot);
}
